#include <ros/ros.h>
#include <robot_audio/robot_iat.h>
#include <robot_audio/Collect.h>
#include <robot_audio/robot_tts.h>
#include <actionlib/client/simple_action_client.h>
#include "move_base_msgs/MoveBaseAction.h"
#include <relative_move/SetRelativeMove.h>
#include <iostream>
#include <string>
#include <fstream>
#include <std_msgs/String.h>
#include <map>
#include <yaml-cpp/yaml.h>
#include <ar_pose/Track.h>
using namespace std;

struct Point {
    float x;
    float y;
    float z;
    float w;
    string name;
    string present;
};

class Interaction {
public:
    Interaction();
    string voice_collect();
    string voice_dictation(const char* filename);
    string voice_tts(const char* text);
    void goto_nav(Point* point);

private:
    ros::NodeHandle n;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>* ac;
    ros::ServiceClient collect_client, dictation_client, tts_client;
};

Interaction::Interaction() {
    collect_client = n.serviceClient<robot_audio::Collect>("voice_collect");
    dictation_client = n.serviceClient<robot_audio::robot_iat>("voice_iat");
    tts_client = n.serviceClient<robot_audio::robot_tts>("voice_tts");
}

string Interaction::voice_collect() {
    ros::service::waitForService("voice_collect");
    robot_audio::Collect srv;
    srv.request.collect_flag = 1;
    if (collect_client.call(srv)) {
        ROS_INFO("语音采集成功，文件路径: %s", srv.response.voice_filename.c_str());
    } else {
        ROS_ERROR("语音采集失败");
    }
    return srv.response.voice_filename;
}

string Interaction::voice_dictation(const char* filename) {
    ros::service::waitForService("voice_iat");
    robot_audio::robot_iat srv;
    srv.request.audiopath = filename;
    if (dictation_client.call(srv)) {
        ROS_INFO("语音听写结果: %s", srv.response.text.c_str());
    } else {
        ROS_ERROR("语音听写失败");
    }
    return srv.response.text;
}

string Interaction::voice_tts(const char* text) {
    ros::service::waitForService("voice_tts");
    robot_audio::robot_tts srv;
    srv.request.text = text;
    if (tts_client.call(srv)) {
        ROS_INFO("语音合成并播放: %s", text);
        system("play -n synth 1 sin 0 vol 0");
        ros::Duration(0.1).sleep();
        string cmd = "aplay " + srv.response.audiopath;
        int ret = system(cmd.c_str());
        if (ret == -1) {
            ROS_ERROR("播放语音文件失败");
        }
        ros::Duration(2.0).sleep();
    } else {
        ROS_ERROR("语音合成失败");
    }
    return srv.response.audiopath;
}

void Interaction::goto_nav(Point* point) {
    ac = new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>("move_base", true);
    ROS_INFO("等待动作服务器启动...");
    ac->waitForServer();
    ROS_INFO("动作服务器已启动，发送目标点: %s", point->name.c_str());
    
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    
    goal.target_pose.pose.position.x = point->x; 
    goal.target_pose.pose.position.y = point->y;
    goal.target_pose.pose.orientation.z = point->z;
    goal.target_pose.pose.orientation.w = point->w;
    ac->sendGoal(goal);
    ac->waitForResult();
    if (ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
        ROS_INFO("导航到达目标点: %s", point->name.c_str());
    } else {
        ROS_WARN("导航未成功到达目标点: %s", point->name.c_str());
    }
    ac->cancelGoal();
    delete ac;
}

class PointManager {
public:
    void readPointsFromYAML(const std::string& filename, Point* points);
    static bool sec;
private:
    ros::NodeHandle n;
};

bool PointManager::sec = false;

void PointManager::readPointsFromYAML(const std::string& filename, Point* points) {
    YAML::Node config = YAML::LoadFile(filename);
    std::cout << "开始加载点位信息" << std::endl;

    PointManager::sec = config[0]["is_start_secondary_location"].as<bool>();

    for (size_t i = 1; i < config.size(); ++i) {
        points[i-1].x = config[i]["x"].as<float>();
        points[i-1].y = config[i]["y"].as<float>();
        points[i-1].z = config[i]["z"].as<float>();
        points[i-1].w = config[i]["w"].as<float>();
        points[i-1].name = config[i]["name"].as<string>();
        points[i-1].present = config[i]["present"].as<string>();
        std::cout << "加载点位信息: " << points[i-1].name << " (x: " << points[i-1].x << ", y: " << points[i-1].y << ")" << std::endl;
        ROS_INFO("加载点位信息: %s (x: %.2f, y: %.2f)", points[i-1].name.c_str(), points[i-1].x, points[i-1].y);
    }

    std::cout << "加载点位信息完成" << std::endl;
}

class ConfigManager {
public:
    std::vector<std::pair<std::string, int>> readConfigFile(const std::string& filename);

private:
    ros::NodeHandle n;
};

std::vector<std::pair<std::string, int>> ConfigManager::readConfigFile(const std::string& filename) {
    std::vector<std::pair<std::string, int>> config;
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法打开文件: " << filename << std::endl;
        return config;
    }

    std::string line;
    while (std::getline(file, line)) {
        size_t delimiterPos = line.find('=');
        if (delimiterPos == std::string::npos) {
            continue;
        }

        std::string parameter = line.substr(0, delimiterPos);
        std::string valueStr = line.substr(delimiterPos + 1);

        try {
            int value = std::stoi(valueStr);
            config.push_back(std::make_pair(parameter, value));
        } catch (const std::invalid_argument& e) {
            std::cerr << "参数 " << parameter << " 的值无效: " << valueStr << std::endl;
        } catch (const std::out_of_range& e) {
            std::cerr << "参数 " << parameter << " 的值超出范围: " << valueStr << std::endl;
        }
    }

    file.close();
    return config;
}

class Robot {
public:
    Robot();
    void start(int argc, char **argv);
private:
    void processVoiceCommand(const std::string& text);
    void handleKnownFace(const std_msgs::String::ConstPtr& msg);
    void handleChargingComplete(const std_msgs::String::ConstPtr& msg);

    Interaction audio;
    PointManager point_manager;
    ConfigManager config_manager;
    Point m_point[7];
    int a, b, c, d, e;
    bool charging_complete;

    ros::Publisher signal_pub;
    ros::Subscriber face_sub;
    ros::Subscriber charging_sub;
};

Robot::Robot() : charging_complete(false) {
    ros::NodeHandle n;
    signal_pub = n.advertise<std_msgs::String>("xinhao", 10);
}

void Robot::start(int argc, char **argv) {
    while (ros::ok()) {
        std::vector<std::pair<std::string, int>> config = config_manager.readConfigFile("/home/bobac3/ros_workspace/order.txt");
        for (const auto& pair : config) {
            if (pair.first == "a") a = pair.second;
            if (pair.first == "b") b = pair.second;
            if (pair.first == "c") c = pair.second;
            if (pair.first == "d") d = pair.second;
            if (pair.first == "e") e = pair.second;
        }
        point_manager.readPointsFromYAML("/home/bobac3/ros_workspace/points.yaml", m_point);

        audio.voice_tts("钉");
        std::string dir = audio.voice_collect();
        std::string text = audio.voice_dictation(dir.c_str());

        processVoiceCommand(text);
    }
}

void Robot::processVoiceCommand(const std::string& text) {
	ros::NodeHandle n;
        ros::ServiceClient relative_move_client;
        ros::ServiceClient ar_track_client;
        relative_move_client = n.serviceClient<relative_move::SetRelativeMove>("relative_move");
        relative_move::SetRelativeMove RelativeMove_data;

    if (text.find("一圈") != string::npos) {
        audio.voice_tts("好的");
        
        int points[] = {a, b, c, d, e};
        for (int i = 0; i < 5; i++) {
            if (PointManager::sec) {
                std::cout << "启用二次定位模式" << std::endl;
                Point p = m_point[points[i]];
                p.x -= 0.3;
                p.z = 0;
                p.w = 1;
                audio.goto_nav(&p);

                // 相对移入
                RelativeMove_data.request.goal.x = 0.3;
                RelativeMove_data.request.global_frame = "odom";
                relative_move_client.call(RelativeMove_data);

                audio.voice_tts(m_point[points[i]].present.c_str());

                // 相对移出
                RelativeMove_data.request.goal.x = -0.3;
                RelativeMove_data.request.global_frame = "odom";
                relative_move_client.call(RelativeMove_data);
            } else {
                std::cout << "启用普通点模式" << std::endl;
                audio.goto_nav(&m_point[points[i]]);
                audio.voice_tts(m_point[points[i]].present.c_str());
            }
        }
        audio.goto_nav(&m_point[5]);
    } else if (text.find("我是谁") != string::npos) {
        audio.voice_tts("好的，让我看一看。");

        // 等待人脸识别话题发布
        boost::shared_ptr<const std_msgs::String> msg_ptr;
        while (!(msg_ptr = ros::topic::waitForMessage<std_msgs::String>("mingzi", ros::Duration(5.0)))) {
            ROS_INFO("等待人脸识别消息发布...");
        }

        face_sub = ros::NodeHandle().subscribe("mingzi", 10, &Robot::handleKnownFace, this);
        ros::spin();
    } else {
        std::vector<std::pair<std::string, int>> locations = {
            {"深圳", 0},
            {"上海", 1},
            {"北京", 2},
            {"广州", 3},
            {"吉林", 4}
        };



        for (const auto& location : locations) {
            if (text.find(location.first) != std::string::npos) {
                audio.voice_tts(("好的，这就带您去" + location.first + "馆。").c_str());


 		if (PointManager::sec) {
		        std::cout << "启用二次定位模式" << std::endl;
		        Point p = m_point[location.second];
		        p.x -= 0.3;
		        p.z = 0;
		        p.w = 1;
		        audio.goto_nav(&p);
		        // 相对移入
		        RelativeMove_data.request.goal.x = 0.3;
		        RelativeMove_data.request.global_frame = "odom";
		        relative_move_client.call(RelativeMove_data);
		        audio.voice_tts(m_point[location.second].present.c_str());
		        // 相对移出
		        RelativeMove_data.request.goal.x = -0.3;
		        RelativeMove_data.request.global_frame = "odom";
		        relative_move_client.call(RelativeMove_data);
		} else {
		    std::cout << "启用普通点模式" << std::endl;
		    audio.goto_nav(&m_point[location.second]);
		    audio.voice_tts(m_point[location.second].present.c_str());
		}
		//去充电
                audio.goto_nav(&m_point[6]);
                std_msgs::String msg;
                msg.data = "start_second_move";
                signal_pub.publish(msg);

                ROS_INFO("等待充电完成信号...");
                charging_sub = ros::NodeHandle().subscribe("charging_status", 10, &Robot::handleChargingComplete, this);
                while (ros::ok() && !charging_complete) {
                    ros::spinOnce();
                    ros::Duration(0.1).sleep();
                }

		/*ar_pose::Track Track_data;
	        Track_data.request.ar_id = 0; // 跟踪0号ar码
	        Track_data.request.goal_dist = 0.4;
	        ar_track_client.call(Track_data); // 调用服务
	        RelativeMove_data.request.goal.x = -0.2;
	        RelativeMove_data.request.global_frame = "odom";
	        relative_move_client.call(RelativeMove_data); // 调用服务
	        std::cout << "定位完成" << std::endl;
		*/
                audio.voice_tts("充电成功");
		charging_complete = false;
                ros::Duration(2.0).sleep();
		//退出来
                RelativeMove_data.request.goal.x = 1;
                RelativeMove_data.request.global_frame = "odom";
                relative_move_client.call(RelativeMove_data);

                audio.goto_nav(&m_point[5]);
                break;
            }
        }
    }
}

void Robot::handleKnownFace(const std_msgs::String::ConstPtr& msg) {
    std::map<std::string, std::string> face_map = {
        {"xtq", "解天骐"},
        {"yyr", "杨元瑞"},
        {"hld", "黄力代"},
        {"zrz", "张瑞芝"}
    };

    std::string face_id = msg->data; // 从消息中提取人脸ID

    if (face_map.find(face_id) != face_map.end()) {
        ROS_INFO("识别到人脸: [%s]", face_id.c_str());
        audio.voice_tts(face_map[face_id].c_str());
    } else {
        ROS_INFO("未知人脸: [%s]", face_id.c_str());
    }
}

void Robot::handleChargingComplete(const std_msgs::String::ConstPtr& msg) {
    if (msg->data == "charging_complete") {
        charging_complete = true;
        ROS_INFO("收到充电完成信号");
    }else{
        charging_complete = false;
    }
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "interaction");
    Robot robot;
    robot.start(argc, argv);
    return 0;
}

